/**
 * Copyright (c) 2019, Freqchip
 *
 * All rights reserved.
 *
 *
 */

/*
 * INCLUDES
 */
#include "jump_table.h"
#include "co_printf.h"
#include "ble_stack.h"
#include "driver_uart.h"

/* FreeRTOS headers */
#include "FreeRTOS.h"
#include "FreeRTOSConfig.h"
#include "task.h"
#include "timers.h"
#include "semphr.h"
#include "event_groups.h"

/*
 * MACROS
 */

/*
 * CONSTANTS
 */

/*
 * TYPEDEFS
 */

/*
 * GLOBAL VARIABLES
 */

/*
 * LOCAL VARIABLES
 */


/*
 * EXTERN FUNCTIONS
 */
void freertos_baseband_restore_done(void);

/*
 * PUBLIC FUNCTIONS
 */

#ifdef __GNUC__
__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __current_sp(void)
{
    uint32_t result;

    __ASM volatile ("MOV %0, sp" : "=r" (result) );
    return(result);
}
#endif  // _GNUC__



/*********************************************************************
 * @fn      app_rtos_entry
 *
 * @brief   RTOS relative entries. User should check FreeRTOSConfig.h for function
 *          implementations.
 *          RTOS_ENTRY_TYPE_INIT - used to initialize RTOS. Two tasks has to
 *              be created here, one is for ble stack, one is for dealing GATT messages
 *              (gatt_msg_handler registed by gatt_add_service will be called inside
 *              this task). The ble stack task has to take the most high priority.
 *          RTOS_ENTRY_TYPE_STACK_PUSH - used by lower layer to inform ble stack task
 *              that a new messge has been generated.
 *          RTOS_ENTRY_TYPE_STACK_YIELD - used by ble stack to pend for new message.
 *          RTOS_ENTRY_TYPE_WAKEUP_RESTORE - used to restore RTOS information after wake
 *              up, especially the OS tick information.
 *          RTOS_ENTRY_TYPE_POST_GATT_MSG - used by HOST layer to send GATT message
 *              to gatt_msg_task.
 *
 * @param   type    - @ref RTOS_ENTRY_TYPE_xx
 *          arg     - depend on entry type
 *
 * @return  None.
 */

__attribute__((section("ram_code")))void app_rtos_entry(uint8_t type, void *arg)
{
    static TaskHandle_t ble_stack_task_handle;
    //static TaskHandle_t ble_gatt_msg_task_handle;
    BaseType_t xHigherPriorityTaskWoken = pdFAIL;

    if(type == RTOS_ENTRY_TYPE_INIT)
    {
        uint32_t *task_handle;

        /* create ble stack task */
        xTaskCreate((TaskFunction_t)ble_stack_schedule, "BLE STACK", 512, NULL, ( configMAX_PRIORITIES - 2 ), &ble_stack_task_handle);
        task_handle = (uint32_t *)ble_stack_task_handle;
        co_printf("ble_stack_task_handle stack start: 0x%08x.\r\n", task_handle[12]);

        /* Start the scheduler. */
        vTaskStartScheduler();
    }
    else if(type == RTOS_ENTRY_TYPE_STACK_PUSH)
    {
        if((__get_PSP() == __current_sp())
           ||(xTaskGetSchedulerState() == taskSCHEDULER_NOT_STARTED))
        {
            xTaskNotify(ble_stack_task_handle, 0, eSetValueWithOverwrite);
        }
        else
        {
            xTaskNotifyFromISR(ble_stack_task_handle, 0, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
            if(xHigherPriorityTaskWoken == pdTRUE)
            {
                taskYIELD();
            }
        }
    }
    else if(type == RTOS_ENTRY_TYPE_STACK_YIELD)
    {
        xTaskNotifyWait(0, 0, 0, portMAX_DELAY);
        //ulTaskNotifyTake(1,portMAX_DELAY);
    }
    else if(type == RTOS_ENTRY_TYPE_WAKEUP_RESTORE)
    {
        freertos_baseband_restore_done();
    }
    else if(type == RTOS_ENTRY_TYPE_POST_GATT_MSG)
    {
        co_printf("RTOS_ENTRY_TYPE_POST_GATT_MSG\r\n");
        //while(xQueueSend(rtos_gatt_msg_queue, arg, 0) != pdTRUE);
    }
}

